
#include "Grid.h"
#include "GridInterpolation.h"

Grid* Grid::vGrid;
Grid::_Particle Grid::Particle;

void Grid::wbSetPos(Vector3& gap){

	startPos = pos - gap/2;
	endPos = pos + gap/2;

	ptcCenter = pos;



}//void SetPos()

int Grid::wbSetnumber(){

	return numParticle = particle.size();

	
}//void Setnumber()

Vector3 Grid::wbInGrid(Vector3& pos){

	Vector3 nor = pos - ptcCenter;
	float dist = nor.length();

	if(dist == 0)
		return Vector3(0,0,0);


	Vector3 Bfield = 3 * nor * nor.dot(mm) -  mm;
	Bfield *= pow(dist, -3) * Magnet::k;


	

	nor.normalize();
	float mn = mm.dot(nor);

	Vector3 retval = -5 * nor * mn * mn * numParticle;

	retval += nor * mm.length() * mm.length() * numParticle;
	retval += 2 * mn * mm * numParticle;

//	retval *= 3/pow(dist, 4);

	retval *= Magnet::k * dist / pow(0.5, 2);

	return retval;


}//void InGrid()

Vector3 Grid::wbInNeighbor(Vector3& pos){


	Vector3 nor = pos - ptcCenter;
	float dist = nor.length();

	/*
	if(dist == 0)
		return Vector3(0,0,0);
		*/

	nor.normalize();

	float mn = mfield.dot(nor);
	
	Vector3 retval = -5 * nor * mn * mn * numParticle;

	retval += nor * mfield.length() * mfield.length() * numParticle;
	retval += 2 * mn * mfield * numParticle;

	retval *= 3 * Magnet::k / pow(dist, 4);

	return retval;

}//Vector3 InGrid(pos)


void Grid::_wbCalculateParticleDistribution() {

	auto end = particle.end();
	Vector3 center;

	int num = particle.size();
	
	for (auto it= particle.begin(); it != end; it++) 
		center += **it;
	center /= num;

	Vector3 var;
	float dist = 0.0f;
	for (auto it= particle.begin(); it != end; it++) {
		Vector3 d = **it - center;
		var += d;
		dist += d.length();
	}
	dist /= num;
}

void Grid::updateNeighborCenter() {

	Vector3 c;
	for (int i=0; i<9; i++) {
		Grid *pGrid = vGrid + neighbor[i];
		c += pGrid->ptcCenter;
	}

	c /= 9.f;
}